#include "CloudOperation.h"

const double Pi = 3.14159265358979323846;

void CloudOperation::CreateCircle3DCloud(const pcl::ModelCoefficients& coefficients, PointCloud<PointXYZ>& OutputCloud) {
	double nx = coefficients.values[4], ny = coefficients.values[5], nz = coefficients.values[6];
	double cx = coefficients.values[0], cy = coefficients.values[1], cz = coefficients.values[2];
	double r = coefficients.values[3];

	double ux = ny, uy = -nx, uz = 0;
	double vx = nx * nz,
		vy = ny * nz,
		vz = -nx * nx - ny * ny;

	double sqrtU = sqrt(ux * ux + uy * uy + uz * uz);
	double sqrtV = sqrt(vx * vx + vy * vy + vz * vz);

	double ux_ = (1 / sqrtU) * ux;
	double uy_ = (1 / sqrtU) * uy;
	double uz_ = (1 / sqrtU) * uz;

	double vx_ = (1 / sqrtV) * vx;
	double vy_ = (1 / sqrtV) * vy;
	double vz_ = (1 / sqrtV) * vz;

	double xi, yi, zi;
	double t = 0;
	double angle = (t / 180.0) * Pi;

	while (t < 360.0)
	{
		xi = cx + r * (ux_ * cos(angle) + vx_ * sin(angle));
		yi = cy + r * (uy_ * cos(angle) + vy_ * sin(angle));
		zi = cz + r * (uz_ * cos(angle) + vz_ * sin(angle));

		t = t + 1;
		angle = (t / 180.0) * Pi;
		OutputCloud.points.push_back(PointXYZ(xi, yi, zi));
	}

}